import dartpy as dart


def main():
    world = dart.simulation.World()

    urdfParser = dart.utils.DartLoader()
    kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
    ground = urdfParser.parseSkeleton("dart://sample/urdf/KR5/ground.urdf")
    world.addSkeleton(kr5)
    world.addSkeleton(ground)
    print("Robot {} is loaded".format(kr5.getName()))

    for i in range(100):
        if i % 10 == 0:
            print(
                "[{}] joint position: {}".format(
                    world.getSimFrames(), kr5.getPositions()
                )
            )
        world.step()


if __name__ == "__main__":
    main()
